//
// Created by ze on 2023/11/2.
//

#include "UPRE_LOCATION.h"
#include "UPRE_GYRO.h"
#include "tim.h"

//全场位姿
Pose Robot_Pose = {0};
//全场定位结构体
Location location = {0};
//是否正在上坡或下坡
bool Slop_Going = false;
//上下坡次数  偶数是将要下坡  奇数是将要上坡
uint8_t Slop_Num = 0;
//坡的长度
#define Slop_len 1295.f
//坡的高度
#define Slop_High 100.f
//上坡走过的距离
float Slop_Cal_len = 0.f;
//记录坡前的坐标
float Slop_len_Compare = 0.f;
//上坡百分比
float Slop_Percent = 0.f;
static uint8_t Go_Num = 0;
//是否可以初始化比较值Slop_len_Compare
static bool Flag_Init = true;
//再次检测上坡
static bool Flag_Again = false;
static uint8_t Tx_buf[24] = {0};

void Encoder_On() {
    //码盘读数
    static int16_t Enc1, Enc2, Last_Enc1, Last_Enc2;
    Enc1 = __HAL_TIM_GET_COUNTER(&htim1);
    Enc2 = -__HAL_TIM_GET_COUNTER(&htim3);
    if (fabsf((float) Enc1 - (float) Last_Enc1) >= 50000.f)
        location.DIS_1 = 0;
    else
        location.DIS_1 = (float) (Enc1 - Last_Enc1); //X轴
    if (fabsf((float) Enc2 - (float) Last_Enc2) >= 50000.f)
        location.DIS_2 = 0;
    else
        location.DIS_2 = (float) -(Enc2 - Last_Enc2); //Y轴

#if Printf_Pose  == 1
    printf("X:%f\tY:%f\tPITCH:%f\tROLL:%f\tYAW:%f\r\n",Robot_Pose.wx,Robot_Pose.wy,Robot_Pose.pitch,Robot_Pose.roll,Robot_Pose.yaw);

#endif


    /******************微分***********************/
    Last_Enc1 = Enc1;
    Last_Enc2 = Enc2;
}

void Location_Init(void) {
    /*************************不要动************************/
    //    location.theta = Angle2Radian(45.f - 1.068663917f -0.355318109f-0.317003402f);/*171/9167*/
    //    location.theta = Angle2Radian(45.f - 1.155153883f -  0.634549086f + 0.774391697f);
    location.theta = Angle2Radian(45.f);
    /*****************************************************/
    location.gyro_radian = Angle2Radian(gyro.Angle_azimuth);
    location.theta1 = Angle2Radian(Tt1) + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2) + location.gyro_radian;
    location.Sin_12 = sinf(Angle2Radian(Tt1 - Tt2));
    location.Sin_21 = sinf(Angle2Radian(Tt2 - Tt1));
    //距离标定法
    //    location.R1 = 100.5f;
    //    location.R2 = 100.5f;
    //    location.fail1 = Angle2Radian(150.f);
    //    location.fail2 = Angle2Radian(36.f);
    //旋转中心标定
    location.Center_X = -194.555f;
    location.Center_Y = 72.475f;
    //    location.RECTANG_X = -0.0132264529f-0.0028152021f;
    //    location.RECTANG_X = 0.f;
    //    location.RECTANG_Y = 0.f;
    //    location.k1 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
    //    location.k2 =   arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  * location.RECTANG_X;
    //    location.k3 = - arm_sin_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_cos_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
    //    location.k4 =   arm_cos_f32(location.theta) * PULSE2DIS_ENCODE2  + arm_sin_f32(location.theta) * PULSE2DIS_ENCODE1  * location.RECTANG_Y;
    //    location.g_theta1 = atan2f(-location.k2,location.k4);
    //    location.g_theta2 = atan2f(-location.k1,location.k3);

    //location.g_pulse_dis1 = location.k1 / (PULSE2DIS_ENCODE1 * arm_sin_f32(location.g_theta2) / location.Sin_21);
    //location.g_pulse_dis2 = location.k2 / (PULSE2DIS_ENCODE2 * (-arm_sin_f32(location.g_theta1)) / location.Sin_21);
}

//全场标定函数
void cal_location(void) {
    static float Last_Pitch = 0.f;
    //逆时针旋转
    Robot_Pose.yaw = gyro.Angle_azimuth;
    location.gyro_radian = Angle2Radian(Robot_Pose.yaw);
    location.ALPHA = location.gyro_radian - location.alphaed;
    location.theta1 = Angle2Radian(Tt1) + location.gyro_radian;
    location.theta2 = Angle2Radian(Tt2) + location.gyro_radian;
    /**************************旋转矩阵**********************************************************/
    //逆时针旋转矩阵
    location.buf_x = ((float) location.DIS_1 * cosf(location.theta) + (float) location.DIS_2 * sinf(location.theta)) *
                     PULSE2DIS_ENCODE1;
    location.buf_y = (-(float) location.DIS_1 * sinf(location.theta) + (float) location.DIS_2 * cosf(location.theta)) *
                     PULSE2DIS_ENCODE2;

    //    Robot_Pose.wx += (float)location.buf_x;
    //    Robot_Pose.wy += (float)location.buf_y;
    //    location.spi_Posx = location.buf_x + location.buf_y * location.RECTANG_X;
    //    location.spi_Posy = location.buf_y + location.buf_x * location.RECTANG_Y;

    //相对标定法：相对 and 全场直线
    //    location.spi_Posx = location.k1 * location.DIS_1 + location.k2 * location.DIS_2;
    //    location.spi_Posy = location.k3 * location.DIS_1 + location.k4 * location.DIS_2;

    //全场
    //location.spi_Posx = (-(float)location.buf_x * arm_sin_f32(location.theta2) + (float)location.buf_y * arm_sin_f32(location.theta1)) / location.Sin_12;
    //location.spi_Posy = (-(float)location.buf_x * arm_cos_f32(location.theta2) + (float)location.buf_y * arm_cos_f32(location.theta1)) / location.Sin_21;

    //全场反解公式
    //location.spi_Posx = ( DIS2 * sint1 - DIS1 * sint2 + _L1 * arm_cos_f32(location.fail1) *
    //        sint2 - _L2 * arm_cos_f32(location.fail2) * sint1 ) / location.Sin_12;
    //location.spi_Posy = ( DIS2 * cost1 - DIS1 * cost2 + _L1 *
    //        arm_cos_f32(location.fail1) * cost2 - _L2 * arm_cos_f32(location.fail2) *
    //        cost1 ) / location.Sin_21;
    ////全场微分
    location.m_spi_Posx = (DIS2 * sint1 - DIS1 * sint2) / location.Sin_12;
    location.m_spi_Posy = (DIS2 * cost1 - DIS1 * cost2) / location.Sin_21;
    //    printf("%f %f %f\r\n",location.m_spi_Posx,location.m_spi_Posy,Robot_Pose.yaw);
    ////积分部分
    location.spi_Posx += location.m_spi_Posx;
    location.spi_Posy += location.m_spi_Posy;
    ////坐标平移变换
    location.Whole_X = location.spi_Posx - location.Center_X + location.Center_X *
                       cosf(location.gyro_radian) - location.Center_Y *
                       sinf(location.gyro_radian);
    location.Whole_Y = location.spi_Posy - location.Center_Y + location.Center_Y *
                       cosf(location.gyro_radian) + location.Center_X *
                       sinf(location.gyro_radian);
    Robot_Pose.wx = location.Whole_X;
    Robot_Pose.wy = location.Whole_Y;
    /******************微分***********************/
    //location.alphaed = location.gyro_radian;
    Last_Pitch = gyro.Angle_pitch;
}

void Clean_Pose(void) {
    location.DIS_1 = 0;
    location.DIS_2 = 0;
    location.buf_x = 0;
    location.buf_y = 0;
    location.spi_Posx = 0;
    location.spi_Posy = 0;
    location.Whole_X = 0;
    location.Whole_Y = 0;
    Robot_Pose.wx = 0;
    Robot_Pose.wy = 0;
}